#include "main.h"
#include "drv_bsp.h"
#include <math.h>
#include <string.h>

/* INT_SRC_TMRA_2_OVF Callback. */
void INT_SRC_TMRA_1_OVF_IrqCallback(void);
/* INT_SRC_TMRA_2_OVF Callback. */
void INT_SRC_TMRA_2_OVF_IrqCallback(void);

#define PI (3.1415926f)
uint16_t u16Polarity[3] = {50, 50, 0};

/**
 * Clarke逆变换：将αβ轴分量转换为三相ABC分量
 * @param alpha α轴分量
 * @param beta  β轴分量
 * @param a     输出A相分量
 * @param b     输出B相分量
 * @param c     输出C相分量
 */
void inverse_clarke(float alpha, float beta, float *a, float *b, float *c)
{
  *a = alpha;
  *b = -0.5f * alpha + 0.8660254f * beta; // 0.8660254 = sqrt(3)/2
  *c = -0.5f * alpha - 0.8660254f * beta;
}

/**
 * Park逆变换：将dq轴分量转换为αβ轴分量
 * @param d     d轴分量
 * @param q     q轴分量
 * @param theta 电角度（弧度）
 * @param alpha 输出α轴分量
 * @param beta  输出β轴分量
 */
void inverse_park(float d, float q, float theta, float *alpha, float *beta)
{
  float sin_val = sinf(theta);
  float cos_val = cosf(theta);

  *alpha = d * cos_val - q * sin_val;
  *beta = d * sin_val + q * cos_val;
}

// 假设dq轴电压指令和当前电角度
float Vd = 0.0f;    // d轴电压指令
float Vq = 50.0f;   // q轴电压指令
float theta = 0.0f; // 电角度（π/2弧度 = 90°）

float Valpha, Vbeta;
float Va, Vb, Vc;

#include "as5600.h"
#include "soft_i2c.h"

#define IIC_SCL_GPIO_Port GPIO_PORT_A
#define IIC_SDA_GPIO_Port GPIO_PORT_A
#define IIC_SCL_Pin GPIO_PIN_03
#define IIC_SDA_Pin GPIO_PIN_04
void scl_high(void) { GPIO_SetPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
void scl_low(void) { GPIO_ResetPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
void sda_high(void) { GPIO_SetPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }
void sda_low(void) { GPIO_ResetPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }
uint8_t scl_read(void) { return GPIO_ReadInputPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
uint8_t sda_read(void) { return GPIO_ReadInputPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }
void delay_nop(uint32_t x)
{
  volatile uint32_t i;
  for (i = 0; i < x; i++)
  {
    __NOP();
  }
}

i2c_ops_t i2c_ops;
as5600_data_t as5600_data;

int32_t main(void)
{
  LL_PERIPH_WE(LL_PERIPH_ALL);
  dsp_drv_init();
  LL_PERIPH_WP(LL_PERIPH_ALL);

  i2c_ops.scl_high = scl_high;
  i2c_ops.scl_low = scl_low;
  i2c_ops.sda_high = sda_high;
  i2c_ops.sda_low = sda_low;
  i2c_ops.sda_read = sda_read;
  i2c_ops.delay = delay_nop;
  i2c_ops.delay_x = 1;

  /* Start timerA */
  TMRA_Start(CM_TMRA_1);

  for (;;)
  {
    //			soft_i2c_readnByte(&i2c_ops,AS5600_ADDRESS_MAG,_stat,as5600_data.read_data,1);
    //			as5600_status_analytical(&as5600_data);
    //			DDL_DelayMS(10);
    //        // Park逆变换：dq → αβ
    //        inverse_park(Vd, Vq, theta, &Valpha, &Vbeta);
    //        // Clarke逆变换：αβ → ABC
    //        inverse_clarke(Valpha, Vbeta, &Va, &Vb, &Vc);
    //
    //				u16Polarity[0] = (uint16_t)(Va + 50.0f);
    //				u16Polarity[1] = (uint16_t)(Vb + 50.0f);
    //				u16Polarity[2] = (uint16_t)(Vc + 50.0f);
    //
    //        if (u16Polarity[0] == 0)
    //            CM_TMRA_2->PCONR1 &= ~(0x01 << 12);
    //        else
    //            CM_TMRA_2->PCONR1 |= 0x01 << 12;
    //        if (u16Polarity[1] == 0)
    //            CM_TMRA_2->PCONR2 &= ~(0x01 << 12);
    //        else
    //            CM_TMRA_2->PCONR2 |= 0x01 << 12;
    //        if (u16Polarity[2] == 0)
    //            CM_TMRA_2->PCONR3 &= ~(0x01 << 12);
    //        else
    //            CM_TMRA_2->PCONR3 |= 0x01 << 12;

    //        CM_TMRA_2->CMPAR1 = (uint16_t)((float)u16Polarity[0] * 0.01 * (float)CM_TMRA_2->PERAR);
    //        CM_TMRA_2->CMPAR2 = (uint16_t)((float)u16Polarity[1] * 0.01 * (float)CM_TMRA_2->PERAR);
    //        CM_TMRA_2->CMPAR3 = (uint16_t)((float)u16Polarity[2] * 0.01 * (float)CM_TMRA_2->PERAR);
  }
}

float kj = 0.01;

/* INT_SRC_TMRA_2_OVF Callback. */
static void INT_SRC_TMRA_2_OVF_IrqCallback(void)
{
  // add your codes here
  //	GPIO_TogglePins(GPIO_PORT_B, GPIO_PIN_00);
}
char str[128];
/* INT_SRC_TMRA_1_OVF Callback. */
static void INT_SRC_TMRA_1_OVF_IrqCallback(void)
{
  // add your codes here
  soft_i2c_readnByte(&i2c_ops, AS5600_ADDRESS_MAG, _stat, as5600_data.read_data, 1);
  as5600_status_analytical(&as5600_data);
  soft_i2c_readnByte(&i2c_ops, AS5600_ADDRESS_MAG, _ang_hi, as5600_data.read_data, 2);
  as5600_angle_analytical(&as5600_data);

  //	theta+=kj;
  //	theta = fmod(theta,2*PI);
  //	GPIO_TogglePins(GPIO_PORT_B, GPIO_PIN_00);

  //	sprintf(str,"%d,%d,%d\n",u16Polarity[0],u16Polarity[1],u16Polarity[2]);

  sprintf(str, "%f\n", as5600_data.angle);
  USART_UART_Trans(CM_USART1, str, strlen(str), 0xffff);
}
